#include "WPILib.h"
#include "PandroidSettings.h"
#include "DriverConsole.h"
#include "DriveSystem.h"
#include "ConveyorMaster.h"
#include "Error.h"
#include "PanShoot.h"

class Pandroid : public RobotBase {
	
private:
	typedef enum {kTeleOp = 0, kAuto = 1, kDisabled} robotMode;
	//Main, physical objects
	DriverConsole *_userConsole;
	NetworkTable *_dash;
	DriveSystem *_driveBase;
	SEM_ID _sID;
	//Virtual objects or constucts
	robotMode _currentMode;
	ConveyorMaster *_convSystem;
	PanShoot *_shooter;
public:
	
	Pandroid() {
		_sID = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
		GetWatchdog().SetEnabled(false);
		_dash = NetworkTable::GetTable("SmartDashboard");
		_userConsole = DriverConsole::GetInstance();
		_convSystem = ConveyorMaster::GetInstance(_dash,_userConsole);
		//dash = NULL;
		//I think it's ok to let the subsections post to the driver station
		//We'll have a function thought that does it - it does make them rather coupled, though
		_driveBase = DriveSystem::GetInstance(_userConsole, _dash);
		_currentMode = kDisabled;
		_shooter = PanShoot::GetInstance(_dash,_userConsole);
	}
	
	~Pandroid() {
		delete _userConsole;
	}
	
	void StartCompetition() {
		printf("Starting competition\n");
		while (true) {
			GetWatchdog().Feed();
			
			if (IsEnabled()) {
				if (IsAutonomous()) {
					Autonomous();
					_currentMode = kAuto;
				} else if (IsOperatorControl()) {
					_currentMode = kTeleOp;
					OperatorControl();
					
				}
			} else
				_currentMode = kDisabled;
			updateDash();
		}
	}
	
	void Autonomous() {
		
	}
	
	void OperatorControl() {
		//_testConv->Set(Relay::kForward);
		//_testMotor->Set(-.25);
		//_driveBase->Teleop(DriveSystem::arcade);
		//_driveBase->DashPost();
		//_convSystem->Teleop(true);
		_shooter->PrintDump();
		//printf("Motor: %f\n", _motor->GetPosition());
	}
	
	void updateDash() {
		_dash->BeginTransaction();
		//_dash->GetDouble("Robot");
		//_dash->PutDouble("test",1);
		//printf("test: %d\n", _dash->GetInt("testing"));
		_dash->EndTransaction();
		//printf("Updated\n");
		//_driveBase->UpdateFromDash();
		//_driveBase->DashPost();
		/*
		static double counter = 0;
		counter += .001;
		//Just to check the network speed
		static bool netBool = false;
		if (netBool != _dash->GetBoolean("testBoolDash")) {
			netBool = !netBool;
		}
		_dash->PutBoolean("netBoolRobot",netBool);
		_dash->PutInt("Counter: ", (int) counter);*/
	}
};

START_ROBOT_CLASS(Pandroid);

